#include <math.h>
#include <chassis_controller/kinematics.h>
#include <chassis_controller/kinematics_tricycle.h>
#include <chassis_controller/pid.h>
#include <string>
namespace chassis_controller{
  
TricycleKinematics::TricycleKinematics(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node):
  Kinematics(node),
  steer_tolerance(0.087266463), // 5 degree
  steer_follow_tolerance(1),
  PID_steer_(node_)
{
}

void TricycleKinematics::init(int pivot_num, pid_param steer_pid_param,SpeedLimiter limiter_liner, SpeedLimiter limiter_ang,SpeedLimiter limiter_steer) //PID instantiation
{
  for (int i =0 ; i< pivot_num ; i++)
  {
    PID_steer_.pid_param_ = steer_pid_param;
    limiter_liner_ = limiter_liner; 
    max_liner_ = limiter_liner_.max_velocity;
    min_liner_ = limiter_liner_.min_velocity;
    max_angular_ = limiter_ang.max_velocity;
    min_angular_ = limiter_ang.min_velocity;
    limiter_steer_ = limiter_steer;   
  }
  start_time_ = node_->now().seconds();  
}

void TricycleKinematics::updateParam(int pivot_num, pid_param steer_pid_param)
{
  PID_steer_.pid_param_ = steer_pid_param;
}

void TricycleKinematics::getCommand(double vx , double vy , double va)
{
  setpoint_.x = vx;
  setpoint_.y = vy;
  setpoint_.a = va;
  DEBUG_OUT_THROTTLE(get_clock(), 
      1000, "current velocity setpoint :" <<setpoint_.x  << setpoint_.y  <<setpoint_.a);
}

void TricycleKinematics::getVelocity(double vx, double vy , double va)
{
  current_vel_.x = vx;
  current_vel_.y = vy;
  current_vel_.a = va;
  DEBUG_OUT_THROTTLE(get_clock(), 
      1000, "current velocity :" <<current_vel_.x   << current_vel_.y   <<current_vel_.a);
}


void TricycleKinematics::update(std::vector<Spivot_value> &pivot_value,
                                   std::vector<Spivot_param> &pivot_param,
                                   std::vector<Spivot_setpoint> &pivot_setpoint, const rclcpp::Duration &period,
                                   bool ready_move)
{  
  pivots_num_ = pivot_value.size();
  steering_ = false ;
  steer_follow_err_ = false;
  bool stop_req = false;

  //启动和已停止状态判断
  drive_request_ = abs(setpoint_.x) > 0.00005 || abs(setpoint_.y) > 0.00005 || abs(setpoint_.a) > 0.00005;
  standstill_ = abs(current_vel_.x) < 0.005 && abs(current_vel_.y) < 0.005 && abs(current_vel_.a) < 0.005;

  if (!drive_request_)
    movement_ = false;
  if (drive_request_)
  {
  steer_cmd_id_valid_ = steer_cmd_id_;
  start_time_ = node_->now().seconds();
  }
  drive_req_trigger_ = drive_request_ && !drive_request_old_;
  drive_request_old_ = drive_request_; 

  double time_elapse = node_->now().seconds() - start_time_;
  if (time_elapse <2 && drive_request_)
    braker_open_ = true;
  else 
    braker_open_ = false; 
  //reset states
  pivot_value[0].steering = false;
  pivot_value[0].steer_follow_err = false;

  /* step1 计算驱动单元理论的速度和角度 */
  //先按驱动单元居中计算
  double temp_vx = setpoint_.x ;
  double temp_vy = setpoint_.a * pivot_param[0].offset_x;
  double temp_liner = sqrt(temp_vx * temp_vx + temp_vy * temp_vy);
  double temp_angle = 0.0;
  if (abs(temp_liner) > 0.0)
    temp_angle = atan2(temp_vy, temp_vx);
  DEBUG_OUT_THROTTLE(get_clock(),
      500, "theoretical value vx,vy,angle,liner:" << 
      temp_vx << "," << temp_vy << "," << temp_angle <<","<<temp_liner);
  


  /*step2 判断取反位*/
  double temp_diff_angle , temp_diff_angle_alt , actual_angle ,max_angle ,min_angle;
  bool negtive_allowed, positive_allowed , invert ,forbidden_invert , suspend_invert , inverted = false;

  max_angle = pivot_param[0].max_angle;
  min_angle = pivot_param[0].min_angle;
  actual_angle = pivot_value[0].steer_position; 
  invert = pivot_setpoint[0].invert;
  forbidden_invert = pivot_setpoint[0].forbidden_invert;
  suspend_invert = pivot_setpoint[0].suspend_invert;

  if (!movement_)
    temp_liner = 0.0;

  if (invert_flag_)
  {
    if (temp_angle>= 0.0)
      temp_angle = temp_angle - PI;
    else 
      temp_angle = temp_angle + PI;
    temp_liner *= -1.0;
  }

  //若果驱动单元不居中，需要换算到实际的位置
  if (abs(tan(temp_angle)) > 0.0000001 && abs(pivot_param[0].offset_y) > 0.0000001 )
  {
    double temp_angle_decentered = 0;//驱动单元变换到居中后的角度

    if (abs(temp_angle - PI_2) < 0.00001) //角度为90度的特殊情况
    {
      if (pivot_param[0].offset_y > 0)
        temp_angle_decentered = -atan(pivot_param[0].offset_x / pivot_param[0].offset_y) + PI;
      else if (pivot_param[0].offset_y < 0)
        temp_angle_decentered = -atan(pivot_param[0].offset_x / pivot_param[0].offset_y);
      //DEBUG_OUT_THROTTLE(get_clock(), 500, "90degree,decentered angle is :" << temp_angle_decentered); 
      DEBUG_STREAM_OUT("90degree,decentered angle is :" << temp_angle_decentered); 
    }
    else if (abs(temp_angle + PI_2) <0.00001) //角度为-90度的特殊情况
    {
      if (pivot_param[0].offset_y > 0)
        temp_angle_decentered = -atan(pivot_param[0].offset_x / pivot_param[0].offset_y);
      else if (pivot_param[0].offset_y < 0)
        temp_angle_decentered = -atan(pivot_param[0].offset_x / pivot_param[0].offset_y) - PI; 
        //DEBUG_OUT_THROTTLE(get_clock(), 500, "-90degree,decentered angle is :" << temp_angle_decentered);
        DEBUG_STREAM_OUT("-90degree,decentered angle is :" << temp_angle_decentered);   
    }
    else if (abs(temp_angle - atan(pivot_param[0].offset_x / pivot_param[0].offset_y)) < 0.00001)//角度为特殊位置，可能出现除0异常
    {
      temp_angle > 0 ? temp_angle_decentered = PI_2 : temp_angle_decentered = -PI_2;
      //DEBUG_OUT_THROTTLE(get_clock(), 500, "specal angle,decentered angle is :" << temp_angle_decentered);
      DEBUG_STREAM_OUT("specal angle,decentered angle is :" << temp_angle_decentered);
    }
    else //正常计算
    {
      temp_angle_decentered = atan(pivot_param[0].offset_x/(pivot_param[0].offset_x / tan(temp_angle) - pivot_param[0].offset_y));
      // if (pivot_param[0].offset_y < 0 && temp_angle < atan(pivot_param[0].offset_x / pivot_param[0].offset_y)){
      //   RCLCPP_INFO_STREAM("offset_y < 0 calculated decentered angle is :" << temp_angle_decentered);
      //   temp_angle_decentered = temp_angle_decentered - PI;        
      // }
        
      // if (pivot_param[0].offset_y > 0 && temp_angle > atan(pivot_param[0].offset_x / pivot_param[0].offset_y)){
      //   RCLCPP_INFO_STREAM("offset_y < 0 calculated decentered angle is :" << temp_angle_decentered);
      //   temp_angle_decentered = temp_angle_decentered + PI;
      // }
        
      //DEBUG_OUT_THROTTLE(get_clock(), 500, "calculated decentered angle is :" << temp_angle_decentered);
      DEBUG_STREAM_OUT("calculated decentered angle is :" << temp_angle_decentered);  
    }
    // RCLCPP_INFO(rclcpp::get_logger("ChassisController"), "[TricycleKinematics] temp_linear: %.4f  temp_angle: %.4f  temp_angle_decentered: %.4f  sin(angle): %.4f  sin(decentered): %.4f",
    //   temp_liner, temp_angle, temp_angle_decentered, sin(temp_angle), sin(temp_angle_decentered));
    temp_liner = temp_liner * sin(temp_angle) / sin(temp_angle_decentered);
    temp_angle = temp_angle_decentered;
    //DEBUG_OUT_THROTTLE(get_clock(), 500, "decentered angle is :" << temp_angle_decentered<<"liner is:"<<temp_liner);
    DEBUG_STREAM_OUT("decentered angle is :" << temp_angle_decentered<<"liner is:"<<temp_liner);
  }


  pivot_value[0].inverted =  invert_flag_; 
  DEBUG_OUT_THROTTLE(get_clock(), 
      500, "invert flag:" << pivot_value[0].inverted << "," << invert_flag_);

  /*取反条件 1 : 强制驱动方向*/
  if (!movement_ && drive_request_ && invert && temp_angle <= 0)//强制左转
  {
    target_invert_flag_ = !invert_flag_;
    inverted = true;
    //DEBUG_OUT_THROTTLE(get_clock(), 500, "invert intentionally");
    DEBUG_STREAM_OUT("[TricycleKinematics]invert intentionally");
  }
  else if (!movement_ && drive_request_ && forbidden_invert && temp_angle >= 0)//强制右转
  {
    target_invert_flag_ = !invert_flag_;
    inverted = true;
    //DEBUG_OUT_THROTTLE(get_clock(), 500, "[TricycleKinematics]forbidden invert");
    DEBUG_STREAM_OUT("[TricycleKinematics]forbidden invert");
  }

  /*取反条件 2 : 超出限值*/
  if (!movement_ && (temp_angle > max_angle || temp_angle < min_angle))
  {
    target_invert_flag_ = !invert_flag_;
    inverted = true;
    //DEBUG_OUT_THROTTLE(get_clock(), 500, "[TricycleKinematics]out of steer range");
    DEBUG_STREAM_OUT("[TricycleKinematics]out of steer range"); 
    DEBUG_STREAM_OUT("[TricycleKinematics]temp_angle:"<<temp_angle);
  }
    
  /*取反条件 3 : 禁止驱动转向*/
  if (suspend_invert && temp_angle > max_angle && movement_)
  {
    temp_angle = pivot_param[0].max_angle;
    //DEBUG_OUT_THROTTLE(get_clock(), 500, "[TricycleKinematics]greater than max_angle");
    DEBUG_STREAM_OUT(
        "[TricycleKinematics]temp_angle is greater than max_angle when driving");
    DEBUG_STREAM_OUT("[TricycleKinematics]temp_angle"<<temp_angle);
  }
  if (suspend_invert && temp_angle < min_angle && movement_)
  {
    temp_angle = pivot_param[0].min_angle;
    DEBUG_OUT_THROTTLE(get_clock(), 
        500, "[TricycleKinematics]temp_angle is smaller than min_angle");
    DEBUG_STREAM_OUT("[TricycleKinematics]temp_angle is smaller than min_angle");
    DEBUG_STREAM_OUT("[TricycleKinematics]temp_angle"<<temp_angle);
  }

  /*取反条件 4 : 使用最短的路径进行反转*/
  temp_diff_angle = temp_angle - actual_angle;  
  DEBUG_STREAM_OUT(
      "[TricycleKinematics] temp_angle:"<<temp_angle<<"actual angle:"<<
      actual_angle<< " temp_diff_angle:"<<temp_diff_angle);
  checkSteerDirection(temp_angle, actual_angle, max_angle, min_angle, positive_allowed, negtive_allowed);
  DEBUG_STREAM_OUT(
    "[TricycleKinematics]shortest way flag:"<<movement_<<","
    <<drive_req_trigger_<<","<<invert<<","<<forbidden_invert<<","<<inverted);
  if (!movement_ && drive_req_trigger_ && !invert && !forbidden_invert && !inverted)
  {       
    //计算第一个方向的偏差角diff_angle
    if (temp_diff_angle >= 0.0)
    {
      if (temp_diff_angle > PI && negtive_allowed || temp_diff_angle <= PI && negtive_allowed && !positive_allowed)
        temp_diff_angle = 2*PI - temp_diff_angle;
    }
    else
    {
      if (temp_diff_angle < -PI && positive_allowed || temp_diff_angle >= -PI && !negtive_allowed && positive_allowed)
        temp_diff_angle = 2*PI + temp_diff_angle;
    }
    
    //计算第二个方向的偏差角alternative diff angle
    temp_diff_angle_alt = normAngle2(temp_angle + PI - actual_angle);
    double temp_angle_alt = normAngle2(temp_angle + PI );
    checkSteerDirection(temp_angle_alt, actual_angle , max_angle , min_angle , positive_allowed ,negtive_allowed);
    if (temp_diff_angle_alt >= 0.0)
    {
      if (temp_diff_angle_alt > PI && negtive_allowed || temp_diff_angle_alt<=PI && negtive_allowed && !positive_allowed)
        temp_diff_angle_alt = 2*PI - temp_diff_angle_alt;
    }
    else
    {
      if (temp_diff_angle_alt < -PI && positive_allowed || temp_diff_angle_alt >= -PI && !negtive_allowed && positive_allowed)
        temp_diff_angle_alt = 2*PI + temp_diff_angle_alt;
    }
    DEBUG_STREAM_OUT("[TricycleKinematics]first diff angle is :" << temp_diff_angle << "," 
        << "second diff angle is :" << temp_diff_angle_alt);    
    //取最短的转舵路径
    if (temp_diff_angle > PI_2 && abs(temp_diff_angle_alt) < abs(temp_diff_angle) /* && !pivot_value[i].inverted */
        && !(normAngle2(temp_angle +PI) > max_angle || normAngle2(temp_angle +PI) < min_angle))
    {
      target_invert_flag_ = !target_invert_flag_;
      DEBUG_STREAM_OUT("[TricycleKinematics] take second angle");
    } 
  }
  
  /*第三步 状态转移   */
  temp_diff_angle = abs(temp_angle - actual_angle);
  if (temp_diff_angle >PI)
    temp_diff_angle = abs(2*PI - temp_diff_angle);
  DEBUG_OUT_THROTTLE(get_clock(), 
      500, "[TricycleKinematics]absolute diff angle is : "<< temp_diff_angle 
      << ",temp_angle=" << temp_angle << ",actual_angle=" << actual_angle);

  //静止起步阶段舵角超出范围，则原地转舵，输出steering状态
  if (temp_diff_angle > steer_tolerance && !movement_ && drive_request_)
  {
    pivot_value[0].steering = true;
    steering_ = true;
    DEBUG_OUT_THROTTLE(get_clock(), 
    500, "[TricycleKinematics]steering flag : "<< steering_);
  }

  //行进过程中舵角超差，置停止请求位，可能是因为转向跟踪过慢
  if (temp_diff_angle > steer_follow_tolerance && movement_)
  {
    pivot_value[0].steer_follow_err = true;
    steer_follow_err_ = true;
      DEBUG_ERROR_OUT(
          "steer_follow_err: " << steer_follow_err_ << ",steer_follow_tolerance=" <<
          steer_follow_tolerance << ",temp_diff_angle=" << temp_diff_angle <<
          ",temp_angle=" << temp_angle << ",actual_angle=" << actual_angle);
  }


  //速度限制
  //DEBUG_OUT_THROTTLE(get_clock(), 500, "[TricycleKinematics]temp_liner before limiter : "<< temp_liner);
  DEBUG_STREAM_OUT("[TricycleKinematics]temp_liner before limiter : "<< temp_liner);
  const double cmd_dt(period.seconds());
  double reduce_factor_liner;
  double max_velocity_ang = max_angular_ * pivot_param[0].offset_x;

  limiter_liner_.max_velocity = abs(setpoint_.x) >= abs(max_velocity_ang) ? abs(setpoint_.x) : abs(max_velocity_ang);
  limiter_liner_.min_velocity =  -limiter_liner_.max_velocity;
  DEBUG_STREAM_OUT("[TricycleKinematics]max_velocity_ang : "<< max_velocity_ang);
  DEBUG_STREAM_OUT("[TricycleKinematics]setpoint_.x : "<< setpoint_.x);
  DEBUG_STREAM_OUT(
      "[TricycleKinematics]limiter_liner_.max_velocity : "<< limiter_liner_.max_velocity);
  // RCLCPP_INFO(rclcpp::get_logger("ChassisController"), "[TricycleKinematics] before limiter  temp_linear: %.4f  orig_max: %.4f  orig_min: %.4f", temp_liner, max_liner_, min_liner_);

  reduce_factor_liner = limiter_liner_.limit(temp_liner,last0_cmd_liner_,last1_cmd_liner_,cmd_dt);
  last1_cmd_liner_ = last0_cmd_liner_;
  last0_cmd_liner_ = temp_liner;

  liner_setpoint_ = temp_liner;
  angle_setpoint_ = temp_angle;
  //DEBUG_OUT_THROTTLE(get_clock(), 500, "[TricycleKinematics]temp_liner after limiter : "<< liner_setpoint_);
  DEBUG_STREAM_OUT(
      "[TricycleKinematics]temp_liner after limiter : "<< liner_setpoint_);
  
  stop_req = steering_ || steer_follow_err_;
  DEBUG_OUT_THROTTLE(get_clock(), 
      1000, 
      "[TricycleKinematics]reqStop flag : "<< stop_req<< ",steering_=" << 
      steering_ << ",steer_follow_err_=" << steer_follow_err_);

  //use last valid value while inverting
  if (target_invert_flag_ != invert_flag_)
  { 
    stop_req = true;    
    angle_setpoint_ = angle_setpoint_last_valid;  

  }
  else if (drive_request_)//save last valid value 
  {
    angle_setpoint_last_valid = angle_setpoint_;  
  }
   
  if (!drive_request_){
    angle_setpoint_ = angle_setpoint_last_valid; 
    target_invert_flag_ = false;
    invert_flag_ = false;
  } //keep angle 
 
  if (target_invert_flag_ != invert_flag_ && standstill_)
  { 
    invert_flag_ = target_invert_flag_;
  } 

  if (!movement_ && drive_request_ && !stop_req)
    movement_ = true;

  /*  转舵控制，分为舵轮和差速单元两种情况 */
  if (pivot_param[0].pivot_type == STEERWHEEL) //舵轮直接把计算的角度赋值
  {
      bool invert_sign = pivot_param[0].steer_feed_invert;

      pivot_setpoint[0].steer_setpoint = invert_sign ? angle_setpoint_ * -1.0f : angle_setpoint_;
      pivot_setpoint[0].velocity_setpoint = liner_setpoint_;

      if (pivot_param[0].joint_param[0].drive_mode == TRACTION)
        pivot_setpoint[0].joint_setpoint[0].velocity_setpoint = pivot_setpoint[0].velocity_setpoint;
      else if (pivot_param[0].joint_param[0].drive_mode == STEER)
        pivot_setpoint[0].joint_setpoint[0].position_setpoint = pivot_setpoint[0].steer_setpoint + (invert_sign ? 1.0f : -1.0f) * pivot_param[0].steer_offset;

      if (pivot_param[0].joint_param[1].drive_mode == TRACTION)
        pivot_setpoint[0].joint_setpoint[1].velocity_setpoint = pivot_setpoint[0].velocity_setpoint;
      else if (pivot_param[0].joint_param[1].drive_mode == STEER)
        pivot_setpoint[0].joint_setpoint[1].position_setpoint = pivot_setpoint[0].steer_setpoint +(invert_sign ? 1.0f : -1.0f) * pivot_param[0].steer_offset;
      DEBUG_OUT_THROTTLE(get_clock(), 
          500,"[TricycleKinematics]steerWheel: steer_setpoint=" << 
          pivot_setpoint[0].steer_setpoint << ",velocity_setpoint=" << 
          pivot_setpoint[0].velocity_setpoint);
  } 
  else if (pivot_param[0].pivot_type == DIFFWHEEL) //如果是差速驱动的单元，则进行转角闭环计算
  {
    /* 转角PID计算 */
    PID_steer_.pid_param_.period = period.seconds();

    double delta_omiga = PID_steer_.positionPID(pivot_value[0].steer_position, angle_setpoint_, false, !drive_request_ );
    DEBUG_OUT_THROTTLE(get_clock(), 
        500, "[TricycleKinematics] drive request flag :" << drive_request_);
    DEBUG_OUT_THROTTLE(get_clock(), 
        500, "[TricycleKinematics] PID setpoint :" << angle_setpoint_ << " ,feedback :" 
        << pivot_value[0].steer_position <<" ,PID output :" << delta_omiga);
    DEBUG_OUT_THROTTLE(get_clock(), 
        500, "[TricycleKinematics] pid internal element :" <<
        PID_steer_.pid_param_.Kp   << PID_steer_.p_term_);

    //限制转角计算的加速度
    const double cmd_dt(period.seconds());
    double reduce_factor_steer;
    reduce_factor_steer = limiter_steer_.limit(delta_omiga,last0_cmd_steer_, last1_cmd_steer_,cmd_dt);
    last1_cmd_steer_ = last0_cmd_steer_;
    last0_cmd_steer_ = delta_omiga;

    //需要限制线速度的输出，转舵为0，限制加速度
    if (!steering_)
    {
      pivot_setpoint[0].joint_setpoint[0].velocity_setpoint = liner_setpoint_ - (setpoint_.a + delta_omiga) * pivot_param[0].radius_ringmount;
      pivot_setpoint[0].joint_setpoint[1].velocity_setpoint = liner_setpoint_ + (setpoint_.a + delta_omiga) * pivot_param[0].radius_ringmount;
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[TricycleKinematics] move:liner_setpoint_ :" << liner_setpoint_);
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[TricycleKinematics] move:joint 0 velocity setpoint:" << 
            pivot_setpoint[0].joint_setpoint[0].velocity_setpoint);
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[TricycleKinematics] move:joint 1 velocity setpoint:" << 
            pivot_setpoint[0].joint_setpoint[1].velocity_setpoint);
    }
    else
    {
      pivot_setpoint[0].joint_setpoint[0].velocity_setpoint = -delta_omiga * pivot_param[0].radius_ringmount;
      pivot_setpoint[0].joint_setpoint[1].velocity_setpoint =  delta_omiga * pivot_param[0].radius_ringmount;
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[TricycleKinematics] steering:liner_setpoint_ :" << liner_setpoint_);
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[TricycleKinematics] steering limit delta_omiga :" << delta_omiga);
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[TricycleKinematics] steering:joint 0 velocity setpoint:" << 
            pivot_setpoint[0].joint_setpoint[0].velocity_setpoint);
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[TricycleKinematics] steering:joint 1 velocity setpoint:" << 
            pivot_setpoint[0].joint_setpoint[1].velocity_setpoint);
    }
    if (!drive_request_)
    {
      pivot_setpoint[0].joint_setpoint[0].velocity_setpoint = 0.0;
      pivot_setpoint[0].joint_setpoint[1].velocity_setpoint = 0.0;
    }
    DEBUG_OUT_THROTTLE(get_clock(), 
        500,"[TricycleKinematics] joint 0 velocity setpoint:"<< 
        pivot_setpoint[0].joint_setpoint[0].velocity_setpoint);
    DEBUG_OUT_THROTTLE(get_clock(), 
        500,"[TricycleKinematics] joint 1 velocity setpoint:"<< 
        pivot_setpoint[0].joint_setpoint[1].velocity_setpoint);
  }      
}


void TricycleKinematics::setCommand( std::vector<Spivot_value> &pivot_value, 
                                        std::vector<Spivot_param> &pivot_param, 
                                        std::vector<Spivot_setpoint> &pivot_setpoint,
                                        const rclcpp::Duration &period) 
{ 
  double MperSec2RadPerSec = 2.0 / pivot_param[0].driving_wheel_diameter;
  
  for (int j=0; j<2; ++j)
  {
    std::string pivot_joint_name = pivot_param[0].joint_param[j].name;
    assert (!pivot_joint_name.empty());
    
    if (pivot_param[0].joint_param[j].drive_mode == TRACTION) //assign angle and velocity if active pivot
    {
      double temp_velocity = 0.0;
      temp_velocity =pivot_setpoint[0].joint_setpoint[j].velocity_setpoint * MperSec2RadPerSec;
      pivot_param[0].joint_param[j].handle.joint_command_interfaces[0].get().set_value(temp_velocity); //rad/s 
      DEBUG_OUT_THROTTLE(get_clock(), 
          500,"[TricycleKinematics] joint["<<pivot_joint_name<<"] velocity setpoint is:"<<temp_velocity);
    }  
    else if (pivot_param[0].joint_param[j].drive_mode == STEER)
    {
      double temp_position;
      temp_position = pivot_setpoint[0].joint_setpoint[j].position_setpoint;
      pivot_param[0].joint_param[j].handle.joint_command_interfaces[1].get().set_value(
        pivot_setpoint[0].joint_setpoint[j].position_setpoint);
      DEBUG_OUT_THROTTLE(get_clock(), 
          500,"[TricycleKinematics] joint["<<pivot_joint_name<<"] position setpoint is:"<<temp_position);
    } 
  }
}

void TricycleKinematics::setSteerCmdId(uint64_t steer_cmd_id) 
{
  steer_cmd_id_ = steer_cmd_id;
}

bool TricycleKinematics::getSteeringFlag()
{
  return steering_;
} 

bool TricycleKinematics::getSteerErrFlag()
{
  return steer_follow_err_;
} 

bool TricycleKinematics::getBrakerFlag()
{
  return braker_open_;
}

uint64_t TricycleKinematics::getSteerCmdId()
{
  return steer_cmd_id_valid_;
}

}
